/**
 * @file talker_CAN.cpp
 * @author circleup (circleup@foxmail.com)
 * @brief LCCAN 数据控制发送节点，发布 CAN_send_data 话题
 * @version v2.1
 * @date 2020-06-25
 * 
 * @copyright Copyright (c) 2020
 * 
 */

#include "ros_can_driver.h"
#include "ros_control_driver.h"

#include "ros/ros.h"

/**
 * @brief CAN 控制指令发送节点
 * 
 * @param argc 
 * @param argv 
 * @return int 
 */
int main(int argc, char **argv)
{
  ros::init(argc, argv, "talker_CAN");

  ROS_INFO("This is talker_CAN");

  ros::NodeHandle n;

  ros::Publisher send = n.advertise<ros_car::VCU_frame>("CAN_send_data", 1000);

  ros::Rate loop_rate(DEF_CAR_MESSAGE_FREQ);

  // BYTE vcu_frame_data_test[8] = {0xF9, 0x00, 0xFF, 0xFF, 0xFC, 0xFF, 0xF0, 0x00};
  BYTE vcu_frame_data_test[8] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};

  ros_car::VCU_frame send_msg;
  send_msg.uID = DEF_CAN_CHANNEL_SELECT;
  send_msg.nSendType = DEF_SEND_TYPE;
  send_msg.bRemoteFlag = 0;
  send_msg.bExternFlag = 0;
  send_msg.nDataLen = 8;
  send_msg.uTimeFlag = 0;

  memcpy(&(send_msg.arryData), &vcu_frame_data_test, 8);
  SetVCUDefaultValue(&send_msg);
  SetVCUPedValue(&send_msg, 0x00);
  SetVCUSTRAngleValue(&send_msg, 0x00);
  SetVCUBreakValue(&send_msg, 0x00);
  SetVCUGearValue(&send_msg, 0x01);

  // for(int i = 0; i < 8; ++i)
  // {
  //     ROS_INFO("send_msg.arrdata[%d] is [0x%x]", i, send_msg.arryData[i]);
  // }

  while (ros::ok())
  {
    send.publish(send_msg);

    loop_rate.sleep();
  }


  return 0;
}